import time


class LowPassFilter(object):

    def __init__(self, Tf) -> None:
        self.Tf = Tf
        self.y_prev = 0.0
        self.timestamp_prev = time.time()

    def calc(self, x):
        timestamp = time.time()
        dt = timestamp - self.timestamp_prev

        if (dt < 0.0 or dt > 0.5):
            dt = 1e-3

        alpha = self.Tf / (self.Tf + dt)
        y = alpha * self.y_prev + (1.0 - alpha) * x
        self.y_prev = y
        self.timestamp_prev = timestamp
        return y


'''

LowPassFilter::LowPassFilter(float time_constant)
    : Tf(time_constant)
    , y_prev(0.0f)
{
    timestamp_prev = _micros();
}


float LowPassFilter::operator() (float x)
{
    unsigned long timestamp = _micros();
    // Serial1.println(timestamp - timestamp_prev);
    float dt = (timestamp - timestamp_prev) * 1e-6f;

    if (dt < 0.0f || dt > 0.5f)
        dt = 1e-3f;

    float alpha = Tf/(Tf + dt);
    float y = alpha*y_prev + (1.0f - alpha)*x;

    y_prev = y;
    timestamp_prev = timestamp;
    return y;
}
'''